#include "quadTree.h"

bool QuadTree::AABB::containsPoint(const Vector2& p) const
{
	if (p._x < min_._x)
		return false;
	if (p._x >= max_._x)
		return false;
	if (p._y < min_._y)
		return false;
	if (p._y >= max_._y)
		return false;
	return true;
}

bool QuadTree::AABB::intersectsAABB(const QuadTree::AABB& other) const
{
	if (min_._x >= other.max_._x)
		return false;
	if (max_._x < other.min_._x)
		return false;
	if (min_._y >= other.max_._y)
		return false;
	if (max_._y < other.min_._y)
		return false;
	return true;
}

QuadTree::~QuadTree()
{
	if (northWest_)
	{
		delete northWest_;
		delete northEast_;
		delete southWest_;
		delete southEast_;
	}
}

bool QuadTree::insert(const Node& p)
{
	if (!boundary_.containsPoint(p.position_))
		return false;

	++numTotalPoints_;

	if (numPoints_ < QT_NODE_CAPACITY)
	{
		points_[numPoints_++] = p;
		return true;
	}
	
	if (northWest_ == NULL)
		subdivide();

	if (northWest_->insert(p)) return true;
    if (northEast_->insert(p)) return true;
    if (southWest_->insert(p)) return true;
    if (southEast_->insert(p)) return true;

	--numTotalPoints_;

	return false;
}

bool QuadTree::subdivide()
{
	Vector2 center = (boundary_.min_ + boundary_.max_) * 0.5f;
	northWest_ = new QuadTree(AABB(boundary_.min_._x, center._x, center._y, boundary_.max_._x));
	northEast_ = new QuadTree(AABB(center, boundary_.max_));
	southWest_ = new QuadTree(AABB(boundary_.min_, center));
	southEast_ = new QuadTree(AABB(center._x, boundary_.max_._x, boundary_.min_._y, center._y));

	/*
	for (int i = 0; i < QT_NODE_CAPACITY; ++i)
	{
		const Vector2& p = points_[i];
		if (northWest_->insert(p)) continue;
		if (northEast_->insert(p)) continue;
		if (southWest_->insert(p)) continue;
		if (southEast_->insert(p)) continue;

		return false;
	}
	numPoints_ = 0;
	*/

	return true;
}

bool QuadTree::queryRange(const QuadTree::AABB& range, std::vector<int>& indices) const
{
	if (!boundary_.intersectsAABB(range))
		return true;

	for (int i = 0; i < numPoints_; ++i)
    {
		if (range.containsPoint(points_[i].position_))
		  indices.push_back(points_[i].index_);
    }

    // Terminate here, if there are no children
    if (northWest_ == NULL)
      return true;

    // Otherwise, add the points from the children
	northWest_->queryRange(range, indices);
	northEast_->queryRange(range, indices);
	southWest_->queryRange(range, indices);
	southEast_->queryRange(range, indices);

	return true;
}
